/*
 * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.
 * Created by Denys Sene, January, 1, 2017.
 * Released under MIT License - see LICENSE file for details.
 */
#ifdef ARDUINO
#include "Arduino.h"
#endif

#include "SimpleKalmanFilter.h"
#include <math.h>

SimpleKalmanFilter::SimpleKalmanFilter(float mea_e, float est_e, float q) {
    _err_measure = mea_e;
    _err_estimate = est_e;
    _q = q;
    _last_estimate = 0;
}

float SimpleKalmanFilter::updateEstimate(float mea) {
    _kalman_gain = _err_estimate / (_err_estimate + _err_measure);
    if (isnan(_kalman_gain))
        _kalman_gain = 0.0001;
    _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);
    if (isnan(_current_estimate))
        _current_estimate = 0;
    _err_estimate = (1.0 - _kalman_gain) * _err_estimate + fabs(_last_estimate - _current_estimate) * _q;
    if (isnan(_err_estimate))
        _err_estimate = 0;
    _last_estimate = _current_estimate;
    return _current_estimate;
}

void SimpleKalmanFilter::setMeasurementError(float mea_e) {
    _err_measure = mea_e;
}

void SimpleKalmanFilter::setEstimateError(float est_e) {
    _err_estimate = est_e;
}

void SimpleKalmanFilter::setProcessNoise(float q) {
    _q = q;
}

float SimpleKalmanFilter::getMeasurementError() const { return _err_measure; }

float SimpleKalmanFilter::getKalmanGain() const {
    return _kalman_gain;
}

float SimpleKalmanFilter::getEstimateError() const {
    return _err_estimate;
}

float SimpleKalmanFilter::getProcessNoise() const {
    return _q;
}